#include <PID_v1.h>

// 定义传感器引脚
#define LEFT_SENSOR 16
#define CENTER_SENSOR 18
#define RIGHT_SENSOR 17

// 定义电机引脚和PID参数
#define MOTOR_A1 5
#define MOTOR_A2 6
#define MOTOR_B1 9
#define MOTOR_B2 10
double Setpoint = 0; // 目标值为0，表示车辆应当保持在中心位置
double Kp = 30;       // 比例系数
double Ki = 5;       // 积分系数
double Kd = 1;       // 微分系数

double Input, Output;
PID pid(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
int leftSensorValue, centerSensorValue, rightSensorValue;
int baseSpeed = 100; // 定义基础速度

void setup() {
  Serial.begin(115200);
  // 配置电机引脚为输出模式
  pinMode(MOTOR_A1, OUTPUT);
  pinMode(MOTOR_A2, OUTPUT);
  pinMode(MOTOR_B1, OUTPUT);
  pinMode(MOTOR_B2, OUTPUT);
  // 配置传感器引脚为输入模式
  pinMode(CENTER_SENSOR, INPUT);
  pinMode(LEFT_SENSOR, INPUT);
  pinMode(RIGHT_SENSOR, INPUT);

  // 初始化PID控制器
  pid.SetMode(AUTOMATIC);
  pid.SetOutputLimits(-255, 255);

  int baseSpeed = 100;

  // 输出PID参数信息
  Serial.println("PID Tuning:");
  Serial.print("Kp: ");
  Serial.println(Kp);
  Serial.print("Ki: ");
  Serial.println(Ki);
  Serial.print("Kd: ");
  Serial.println(Kd);
}

// 计算偏差（根据传感器的值计算小车位置相对于轨道中心的偏移量）
double calculateOffset() {
  // 偏差权重设置
  int leftWeight = -1;   // 左传感器权重（假设左侧为负方向）
  int centerWeight = 0;  // 中间传感器权重
  int rightWeight = 1;   // 右传感器权重（假设右侧为正方向）

  // 读取传感器值
  leftSensorValue = digitalRead(LEFT_SENSOR);
  centerSensorValue = digitalRead(CENTER_SENSOR);
  rightSensorValue = digitalRead(RIGHT_SENSOR);

  // 计算偏差
  double offset = leftSensorValue * leftWeight + centerSensorValue * centerWeight + rightSensorValue * rightWeight;
  
  return offset;
}

// 将PID输出应用于电机控制
void applyMotorOutput(double output) {
  // 将PID输出映射到电机速度差
  int speedDifference = constrain(output, -255, 255); // 限幅到合理范围

  // 设置左右轮的转速
  int leftSpeed = baseSpeed;  // 你可以根据需要设置左轮的基础速度
  int rightSpeed = baseSpeed; // 你可以根据需要设置右轮的基础速度

  // 根据速度差调整左右轮的速度
  int leftPWM = constrain(leftSpeed + speedDifference, 0, 255);
  int rightPWM = constrain(rightSpeed - speedDifference, 0, 255);

  setMotorSpeed(leftPWM, rightPWM);

  

}


void loop() {
  // 计算偏差
  Input = calculateOffset();

  // 调用PID控制器
  pid.Compute();

  // 将PID输出应用于电机控制
  applyMotorOutput(Output);
//
//  // 恢复轮子速度到初始速度
//  setMotorSpeed(baseSpeed, baseSpeed);

    // 输出调试信息
  Serial.print("Offset: ");
  Serial.println(Input);
  Serial.print("PID Output: ");
  Serial.println(Output);
  delay(100); // 可根据需要调整延迟时间
}

void setMotorSpeed(int leftSpeed, int rightSpeed) {
  analogWrite(MOTOR_A1, leftSpeed);
  analogWrite(MOTOR_A2, 0);
  analogWrite(MOTOR_B1, rightSpeed);
  analogWrite(MOTOR_B2, 0);
  
  Serial.print("leftSpeed: ");
  Serial.println(leftSpeed);
  Serial.print("rightSpeed: ");
  Serial.println(rightSpeed);
}
